﻿using System;
using System.Collections.Generic;
using System.ComponentModel;
using System.Data;
using System.Drawing;
using System.Linq;
using System.Text;
using System.Windows.Forms;
using System.Threading;
using System.IO;
using System.Diagnostics;

namespace MotorGUI
{
    public partial class MainForm : Form
    {
        static uint canHandle;
        Thread RXThread;
        bool RXActive;
        const int CANBaseAddr = 0x0010000;
        private Waveform waveForm1 = new Waveform();
        private Waveform waveForm2 = new Waveform();

        // fliters
        FilterMA FilterPower = new FilterMA();
        FilterMA FilterCurrent = new FilterMA();
        FilterMA FilterForce = new FilterMA();
        FilterMA FilterMom = new FilterMA();

        // data
        SDataStructure DataStructure = new SDataStructure();

        private bool GoPWM = false;
        private int GoPWMCounter = 0;

        public struct SDataStructure
        {
            public int ReceivedPackets;
            public int ReceiveErrors;
            public int RXThreadTicks;

            public int TransmitPackets;
            public int TransmitErrors;

            // data
            // Sensors - I/O Board
            public int StateActual;
            public int ModeActual;
            public float Voltage;
            public float Current1;
            public float Power1;
            public float PWMOut1;
            public float PWMOut2;
            public float SpeedRPM1;
            public float MomentNM;
            public float Force1;
            public float LoopTime;
            public float LoopTimeMAX;
            public double TotalTime;

            public float OmegaZ;
            public float Yaw;     
            public float MagX;
            public float MagY;
            public float MagZ;
            public float MagYaw;

            // filtered
            public float Current1FLT;
            public float Power1FLT;
            public float Force1FLT;
            public float Moment1FLT;
        };

        public MainForm()
        {
            InitializeComponent();
        }

        private void MainForm_Load(object sender, EventArgs e)
        {
            toolStripStatusLabelCANAdapter.Text = "No adapters";

            // find adapters
            StringBuilder buf = new StringBuilder(32);
            int cnt;

            // Fill the listbox with data
            cnt = LAWICEL.canusb_getFirstAdapter(buf, 32);
            if (cnt > 0) toolStripStatusLabelCANAdapter.Text = "Adapter found: " + buf.ToString();

            // wavefrm init
            waveForm1.Init(1000, 10, Color.Green, Color.LightSkyBlue); // PWMOut1/Current1
            waveForm2.Init(1000, 10, Color.Yellow, Color.Red); // PWMOut2/Current2

            // init filters
            FilterPower.Init(50, 0);
            FilterCurrent.Init(50, 0);
            FilterForce.Init(50, 0);
            FilterMom.Init(50, 0);
        }

        private void buttonOpen_Click(object sender, EventArgs e)
        {
            canHandle = LAWICEL.canusb_Open(IntPtr.Zero,
                                        LAWICEL.CAN_BAUD_1M,
                                        LAWICEL.CANUSB_ACCEPTANCE_CODE_ALL,
                                        LAWICEL.CANUSB_ACCEPTANCE_MASK_ALL,
                                        0);

            if (canHandle == 0) MessageBox.Show("Open Failed");
            else
            {
                // OK
                buttonOpen.Enabled = false;
                buttonClose.Enabled = true;

                // init structures
                DataStructure = new SDataStructure();

                // create receive thread
                RXActive = true;
                RXThread = new Thread(new ThreadStart(RXThreadProc));
                //RXThread.Priority = ThreadPriority.AboveNormal;
                RXThread.IsBackground = true;
                RXThread.Start();

                timerGUI.Enabled = true;
            }
        }

        private void buttonClose_Click(object sender, EventArgs e)
        {
            timerGUI.Enabled = false;

            // Close the CANUSB
            int res = LAWICEL.canusb_Close(canHandle);
            if (LAWICEL.ERROR_CANUSB_OK == res)
            {
                // OK
                buttonOpen.Enabled = true;
                buttonClose.Enabled = false;
                RXActive = false;
            }
            else
            {
                MessageBox.Show("Close Failed");
            }
        }

        void RXThreadProc()
        {
            // open log file
            StreamWriter logFile = File.CreateText(Application.StartupPath + "\\LogFile.txt");
            logFile.WriteLine("TimeSec TotalTime Voltage PWM1 PWM2 Current1 Power1 Force1 Torque1 RPM1 Omega Yaw MagYaw");
            //logFile.WriteLine("TimeSec Omega Yaw MagX MagY MagZ");
            Stopwatch timer = new Stopwatch();
            timer.Start();

            while (RXActive)
            {
                // Read one CAN message
                LAWICEL.CANMsg msg = new LAWICEL.CANMsg();

                int rv = LAWICEL.canusb_Read(canHandle, out msg);
                if (LAWICEL.ERROR_CANUSB_OK == rv)
                {
                    // data OK, convert to float
                    byte[] bytes = BitConverter.GetBytes(msg.data);
                    float data = BitConverter.ToSingle(bytes, 0);
                    double dataDbl = BitConverter.ToDouble(bytes, 0);

                    lock (this)
                    {
                        // IOBoard
                        uint baseAddr = CANBaseAddr;
                        if (msg.id == 0x300 + baseAddr) DataStructure.SpeedRPM1 = data;
                        if (msg.id == 0x301 + baseAddr)
                        {
                            float M = (data - 0.983f) / (1.84f);
                            // kg to Nm
                            M = M * 9.81f / 10;
                            DataStructure.MomentNM = M;
                            DataStructure.Moment1FLT = (float)FilterMom.AddPoint(M);
                        }
                        if (msg.id == 0x302 + baseAddr)
                        {
                            float F = (data - 0.363f) / (0.958f);
                            DataStructure.Force1 = F;
                            DataStructure.Force1FLT = (float)FilterForce.AddPoint(F);
                        }
                        if (msg.id == 0x305 + baseAddr) DataStructure.StateActual = (int)data;
                        if (msg.id == 0x306 + baseAddr) DataStructure.ModeActual = (int)data;
                        if (msg.id == 0x311 + baseAddr) DataStructure.Voltage = data;
                        if (msg.id == 0x310 + baseAddr)
                        {
                            DataStructure.Current1 = data;
                            DataStructure.Current1FLT = (float)FilterCurrent.AddPoint(data);
                        }
                        if (msg.id == 0x312 + baseAddr)
                        {
                            DataStructure.Power1 = data;
                            DataStructure.Power1FLT = (float)FilterPower.AddPoint(data);
                        }
                        if (msg.id == 0x315 + baseAddr) DataStructure.PWMOut1 = data;
                        if (msg.id == 0x316 + baseAddr) DataStructure.PWMOut2 = data;
                        if (msg.id == 0x350 + baseAddr) DataStructure.LoopTime = data;
                        if (msg.id == 0x351 + baseAddr) DataStructure.TotalTime = dataDbl;
                        if (msg.id == 0x352 + baseAddr) DataStructure.LoopTimeMAX = data;

                        // IMU Board
                        uint IMUbaseAddr = 0x030000;
                        if (msg.id == 0x302 + IMUbaseAddr) DataStructure.OmegaZ = -data;
                        if (msg.id == 0x312 + IMUbaseAddr) DataStructure.Yaw = data;
                        if (msg.id == 0x340 + IMUbaseAddr) DataStructure.MagX = data;
                        if (msg.id == 0x341 + IMUbaseAddr) DataStructure.MagY = data;
                        if (msg.id == 0x342 + IMUbaseAddr)
                        {
                            DataStructure.MagZ = data;
                            double magYaw = Math.Atan2(DataStructure.MagY - 0.0867f, DataStructure.MagZ + 0.29f) / 6.28318548f;
                            magYaw = (magYaw - Math.Floor(magYaw)) * 6.28318548f;
                            DataStructure.MagYaw = (float)magYaw;
                        }

                        DataStructure.ReceivedPackets++;
                    }

                    // log data
                    double currentMS = (double)timer.ElapsedTicks/(double)Stopwatch.Frequency * 1000.0;
                    // Log on PWMOut1 (last chunk)
                    if (msg.id == 0x315 + CANBaseAddr)
                    {
                        string line = string.Format("{0} {1} {2} {3} {4} {5} {6} {7} {8} {9} {10} {11} {12}", currentMS / 1000, DataStructure.TotalTime, DataStructure.Voltage, DataStructure.PWMOut1, DataStructure.PWMOut2, DataStructure.Current1FLT, DataStructure.Power1FLT, DataStructure.Force1FLT, DataStructure.Moment1FLT, DataStructure.SpeedRPM1, DataStructure.OmegaZ, DataStructure.Yaw, DataStructure.MagYaw );
                        logFile.WriteLine(line);
                        // MAG Save
                        //string line = string.Format("{0} {1} {2} {3} {4} {5}", currentMS / 1000, DataStructure.OmegaZ, DataStructure.Yaw, DataStructure.MagX, DataStructure.MagY, DataStructure.MagZ);
                        //logFile.WriteLine(line);

                        // send to waveforms
                        SWFData[] newData = new SWFData[1];
                        newData[0].D1 = DataStructure.PWMOut1-1000;
                        newData[0].D2 = DataStructure.Current1;
                        waveForm1.AddPoints(newData);
                        newData[0].D1 = DataStructure.PWMOut2-1000;
                        newData[0].D2 = 0;// DataStructure.Current2;
                        waveForm2.AddPoints(newData);
                    }

                    // Slow logging
                    /*if (msg.id == 0x351 + CANBaseAddr)
                    {
                        // double
                        string line = string.Format("{0} {1} {2}", currentMS, msg.id - CANBaseAddr - 0x300, dataDbl);
                        logFile.WriteLine(line);
                    }
                    else
                    {
                        // float
                        string line = string.Format("{0} {1} {2}", currentMS, msg.id - CANBaseAddr - 0x300, data);
                        logFile.WriteLine(line);
                    }*/
                }
                else if (LAWICEL.ERROR_CANUSB_NO_MESSAGE == rv)
                {
                    // no messages, sleep
                    Thread.Sleep(1);
                    lock (this)
                    {
                        DataStructure.RXThreadTicks++;
                    }
                }

                else
                {
                    // error!
                    lock (this)
                    {
                        DataStructure.ReceiveErrors++;
                    }
                }
            }

            logFile.Close();
        }

        public void SendCANMessage(uint id, float testVar)
        {
            // Send one message with extended id
            LAWICEL.CANMsg msg = new LAWICEL.CANMsg();
            msg.id = id;
            msg.len = 8;
            msg.flags = LAWICEL.CANMSG_EXTENDED;
            byte[] data = new byte[8];
            BitConverter.GetBytes(testVar).CopyTo(data, 0);
            msg.data = BitConverter.ToUInt64(data, 0);
            int rv = LAWICEL.canusb_Write(canHandle, ref msg);
            if (LAWICEL.ERROR_CANUSB_OK == rv)
            {
                // OK
                lock (this)
                {
                    DataStructure.TransmitPackets++;
                }
            }
            else if (LAWICEL.ERROR_CANUSB_TX_FIFO_FULL == rv)
            {
                lock (this)
                {
                    DataStructure.TransmitErrors++;
                }
            }
            else
            {
                lock (this)
                {
                    DataStructure.TransmitErrors++;
                }
            }
        }

        private void timerGUI_Tick(object sender, EventArgs e)
        {
            // update GUI
            // 1. get copy
            SDataStructure copyStruct;
            lock (this)
            {
                copyStruct = DataStructure;
            }

            // 2. set values
            textBoxTXRXFrames.Text = string.Format("{0}/{1}", copyStruct.TransmitPackets, copyStruct.ReceivedPackets);
            textBoxTXRXErrors.Text = string.Format("{0}/{1}", copyStruct.TransmitErrors, copyStruct.ReceiveErrors);
            textBoxRXTicks.Text = copyStruct.RXThreadTicks.ToString();


            textBoxStateActual.Text = string.Format("{0}", copyStruct.StateActual);
            textBoxModeActual.Text = string.Format("{0}", copyStruct.ModeActual);
            textBoxCurrent1.Text = string.Format("{0:f1} A", copyStruct.Current1FLT);
            //textBoxCurrent1.Text = string.Format("{0:f1} A", copyStruct.Current1);
            textBoxPower.Text = string.Format("{0:f1} W", copyStruct.Power1FLT);
            //textBoxPower.Text = string.Format("{0:f1} W", copyStruct.Power1);
            textBoxForceN.Text = string.Format("{0:f3} kg", copyStruct.Force1FLT);
            //textBoxForceN.Text = string.Format("{0:f1} g", copyStruct.Force1);
            textBoxMomentNM.Text = string.Format("{0:f3} Nm", copyStruct.Moment1FLT);
            textBoxSpeedRPM.Text = string.Format("{0:f1} rpm", copyStruct.SpeedRPM1);
            textBoxBattVoltage.Text = string.Format("{0:f1} V", copyStruct.Voltage);
            textBoxPWMOut1.Text = string.Format("{0:f1}", copyStruct.PWMOut1);
            textBoxPWMOut2.Text = string.Format("{0:f1}", copyStruct.PWMOut2);
            textBoxLoopMS.Text = string.Format("{0:f2} ms", copyStruct.LoopTime);
            textBoxLoopMAXMS.Text = string.Format("{0:f2} ms", copyStruct.LoopTimeMAX);
            textBoxTotalTime.Text = string.Format("{0:f2} s", copyStruct.TotalTime);

            double RadS1 = copyStruct.SpeedRPM1 / 60 * 2 * Math.PI;
            double mechPower1 = RadS1 * copyStruct.Moment1FLT;
            textBoxPowerMech1.Text = string.Format("{0:f1} W", mechPower1);
            double efficiency1 = mechPower1/copyStruct.Power1FLT * 100;
            textBoxEff1.Text = string.Format("{0:f1} %", efficiency1);

            textBoxOmegaZ.Text = string.Format("{0:f1} deg/s", copyStruct.OmegaZ / Math.PI * 180);
            textBoxYaw.Text = string.Format("{0:f1} deg", copyStruct.Yaw / Math.PI * 180);
            textBoxMagX.Text = string.Format("{0:f3}", copyStruct.MagX);
            textBoxMagY.Text = string.Format("{0:f3}", copyStruct.MagY);
            textBoxMagZ.Text = string.Format("{0:f3}", copyStruct.MagZ);
            
            textBoxMagYaw.Text = string.Format("{0:f3}", copyStruct.MagYaw);

            // update mode
            int mode = (int)copyStruct.ModeActual;
            switch (mode)
            {
                case 0: //off
                    pictureOff.BackColor = Color.Lime;
                    picturePWMMode.BackColor = Color.Red;
                    pictureOmegaMode.BackColor = Color.Red;
                    pictureYawMode.BackColor = Color.Red;
                    SetMinMaxPWM1Ref(false, 1000, 2000);
                    SetMinMaxPWM2Ref(false, 1000, 2000);
                    SetMinMaxOmegaRef(false, -3.0, 3.0);
                    buttonOff.Enabled = true;
                    buttonPWMMode.Enabled = true;
                    buttonOmegaMode.Enabled = true;
                    buttonYawMode.Enabled = true;
                    break;

                case 1: // PWMMode
                    pictureOff.BackColor = Color.Red;
                    picturePWMMode.BackColor = Color.Lime;
                    pictureOmegaMode.BackColor = Color.Red;
                    pictureYawMode.BackColor = Color.Red;
                    SetMinMaxPWM1Ref(true, 1000, 2000);
                    SetMinMaxPWM2Ref(true, 1000, 2000);
                    SetMinMaxOmegaRef(false, -3.0, 3.0);
                    buttonOff.Enabled = true;
                    buttonPWMMode.Enabled = false;
                    buttonOmegaMode.Enabled = false;
                    buttonYawMode.Enabled = false;
                    break;

                case 2: // OmegaMode
                    pictureOff.BackColor = Color.Red;
                    picturePWMMode.BackColor = Color.Red;
                    pictureOmegaMode.BackColor = Color.Lime;
                    pictureYawMode.BackColor = Color.Red;
                    SetMinMaxPWM1Ref(true, 1000, 2000);
                    SetMinMaxPWM2Ref(false, 1000, 2000);
                    SetMinMaxOmegaRef(true, -3.0, 3.0);
                    buttonOff.Enabled = true;
                    buttonPWMMode.Enabled = false;
                    buttonOmegaMode.Enabled = false;
                    buttonYawMode.Enabled = false;
                    break;

                case 3: // YawMode
                    pictureOff.BackColor = Color.Red;
                    picturePWMMode.BackColor = Color.Red;
                    pictureOmegaMode.BackColor = Color.Red;
                    pictureYawMode.BackColor = Color.Lime;
                    SetMinMaxPWM1Ref(true, 1000, 2000);
                    SetMinMaxPWM2Ref(false, 1000, 2000);
                    SetMinMaxOmegaRef(true, -3.0, 3.0);
                    buttonOff.Enabled = true;
                    buttonPWMMode.Enabled = false;
                    buttonOmegaMode.Enabled = false;
                    buttonYawMode.Enabled = false;
                    break;

                default:
                    pictureOff.BackColor = Color.Red;
                    picturePWMMode.BackColor = Color.Red;
                    pictureOmegaMode.BackColor = Color.Red;
                    pictureYawMode.BackColor = Color.Red;
                    SetMinMaxPWM1Ref(false, 1000, 2000);
                    SetMinMaxPWM2Ref(false, 1000, 2000);
                    SetMinMaxOmegaRef(false, -5.0, 5.0);
                    break;
            }

            // update status
            int status = copyStruct.StateActual;
            switch (status)
            {
                case 0:
                    pictureStatus.BackColor = Color.DimGray;
                    buttonGoOffline.Enabled = false;
                    buttonGoOnline.Enabled = true;
                    buttonAckError.Enabled = false;
                    break;
                case 1:
                    pictureStatus.BackColor = Color.Lime;
                    buttonGoOffline.Enabled = true;
                    buttonGoOnline.Enabled = false;
                    buttonAckError.Enabled = false;
                    break;
                case 2:
                    pictureStatus.BackColor = Color.Red;
                    buttonGoOffline.Enabled = false;
                    buttonGoOnline.Enabled = false;
                    buttonAckError.Enabled = true;
                    break;
            }

            // 3. read inputs
            float PWMRef1 = (float)GetPWM1Ref();
            float PWMRef2 = (float)GetPWM2Ref();
            float OmegaRef = (float)GetOmegaRef();
            float YawRef = (float)GetOmegaRef();

            // 4. send messages
            bool silentMode = false;
            if (!silentMode)
            {
                uint baseAddr = CANBaseAddr;
                SendCANMessage(0x0120 + baseAddr, PWMRef1);
                SendCANMessage(0x0121 + baseAddr, PWMRef2);
                SendCANMessage(0x0140 + baseAddr, OmegaRef);
                SendCANMessage(0x0141 + baseAddr, YawRef);
            }

            // 5. Read status (reset error???)
            //int bla = LAWICEL.canusb_Status(canHandle);

            // draw waveforms
            waveForm1.Draw(panelWaveform1);
            waveForm2.Draw(panelWaveform2);

            // "GO PWM"
            if (GoPWM)
            {
                GoPWMCounter--;
                if (GoPWMCounter < 0)
                {
                    GoPWMCounter = 100; // 100/100 = 1Hz
                    if (GetPWM1Ref() > 1000)
                    {
                        double pwmRef = GetPWM1Ref();
                        pwmRef -= 25;
                        if (pwmRef < 1000) pwmRef = 1000;
                        trackBarPWM1Ref.Value = (int)pwmRef;
                    }
                    else
                    {
                        trackBarPWM1Ref.Value = 1000;
                        GoPWM = false;
                        buttonGo.Enabled = true;
                    }
                }
            }
        }



        private void buttonModeSelect_Click(object sender, EventArgs e)
        {
            uint modeAddress = CANBaseAddr + 0x0101;

            if (sender == buttonOff)
            {
                ResetRefs();
                SendCANMessage(modeAddress, 0);
            }
            else if (sender == buttonPWMMode) SendCANMessage(modeAddress, 1);
            else if (sender == buttonOmegaMode) SendCANMessage(modeAddress, 2);
            else if (sender == buttonYawMode) SendCANMessage(modeAddress, 3);
        }     

        private void buttonStateSelect_Click(object sender, EventArgs e)
        {
            uint stateAddress = CANBaseAddr + 0x0100;
            uint ackErrorAddress = CANBaseAddr + 0x0102;

            if (sender == buttonGoOffline) SendCANMessage(stateAddress, 0);
            else if (sender == buttonGoOnline) SendCANMessage(stateAddress, 1);
            else if (sender == buttonAckError)
            {
                ResetRefs();
                SendCANMessage(ackErrorAddress, 1);
            }
        }

        private void ResetRefs()
        {
            // reset Refs
            trackBarPWM1Ref.Value = trackBarPWM1Ref.Minimum;
            trackBarPWM2Ref.Value = trackBarPWM2Ref.Minimum;
            trackBarOmegaRef.Value = 0;
        }

        private double VALIRefMin = 0;
        private double VALIRefMax = 0;
        
        // PWM1 ref
        private void SetMinMaxPWM1Ref(bool enable, double min, double max)
        {
            if (enable == true)
            {
                trackBarPWM1Ref.Enabled = true;
                textBoxPWM1RefMin.Enabled = true;
                textBoxPWM1RefVal.Enabled = true;
                textBoxPWM1RefMax.Enabled = true;
                trackBarPWM1Ref.Minimum = (int)min;
                trackBarPWM1Ref.Maximum = (int)max;
                textBoxPWM1RefMin.Text = min.ToString();
                textBoxPWM1RefMax.Text = max.ToString();
            }
            else
            {
                trackBarPWM1Ref.Enabled = false;
                textBoxPWM1RefMin.Enabled = false;
                textBoxPWM1RefVal.Enabled = false;
                textBoxPWM1RefMax.Enabled = false;
            }
        }
        private double GetPWM1Ref()
        {
            double refValue = (double)trackBarPWM1Ref.Value;
            textBoxPWM1RefVal.Text = refValue.ToString();

            return refValue;
        }
        private void textBoxPWM1RefVal_Click(object sender, EventArgs e)
        {
            trackBarPWM1Ref.Value = trackBarPWM1Ref.Minimum;
        }

        // PWM2 ref
        private void SetMinMaxPWM2Ref(bool enable, double min, double max)
        {
            if (enable == true)
            {
                trackBarPWM2Ref.Enabled = true;
                textBoxPWM2RefMin.Enabled = true;
                textBoxPWM2RefVal.Enabled = true;
                textBoxPWM2RefMax.Enabled = true;
                trackBarPWM2Ref.Minimum = (int)min;
                trackBarPWM2Ref.Maximum = (int)max;
                textBoxPWM2RefMin.Text = min.ToString();
                textBoxPWM2RefMax.Text = max.ToString();
            }
            else
            {
                trackBarPWM2Ref.Enabled = false;
                textBoxPWM2RefMin.Enabled = false;
                textBoxPWM2RefVal.Enabled = false;
                textBoxPWM2RefMax.Enabled = false;
            }
        }
        private double GetPWM2Ref()
        {
            double refValue = (double)trackBarPWM2Ref.Value;
            textBoxPWM2RefVal.Text = refValue.ToString();

            return refValue;
        }
        private void textBoxPWM2RefVal_Click(object sender, EventArgs e)
        {
            trackBarPWM2Ref.Value = trackBarPWM2Ref.Minimum;
        }

        // Omega Ref
        private void SetMinMaxOmegaRef(bool enable, double min, double max)
        {
            if (enable == true)
            {
                trackBarOmegaRef.Enabled = true;
                textBoxOmegaRefMin.Enabled = true;
                textBoxOmegaRefVal.Enabled = true;
                textBoxOmegaRefMax.Enabled = true;
                trackBarOmegaRef.Minimum = -1000;
                trackBarOmegaRef.Maximum = 1000;
                textBoxOmegaRefMin.Text = min.ToString();
                textBoxOmegaRefMax.Text = max.ToString();

                VALIRefMin = min;
                VALIRefMax = max;
            }
            else
            {
                trackBarOmegaRef.Enabled = false;
                textBoxOmegaRefMin.Enabled = false;
                textBoxOmegaRefVal.Enabled = false;
                textBoxOmegaRefMax.Enabled = false;
            }
        }
        private double GetOmegaRef()
        {
            double refValue = (double)trackBarOmegaRef.Value / (double)trackBarOmegaRef.Maximum;
            refValue = refValue * VALIRefMax;
            textBoxOmegaRefVal.Text = refValue.ToString();

            return refValue;
        }
        private void textBoxOmegaRefVal_Click(object sender, EventArgs e)
        {
            trackBarOmegaRef.Value = 0;
        }

        private void buttonGo_Click(object sender, EventArgs e)
        {
            GoPWM = true;
            buttonGo.Enabled = false;
        }

        private void buttonSaveParams_Click(object sender, EventArgs e)
        {
            float Pomega = float.Parse(textBoxPOmega.Text);
            float Iomega = float.Parse(textBoxIomega.Text);

            SendCANMessage(CANBaseAddr + 0x0150, Pomega);
            SendCANMessage(CANBaseAddr + 0x0151, Iomega);
        } 
    }
}
